/**********************************************************************

**********************************************************************/

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <syslog.h>

// opencv includes
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

// klt includes
#include "pnmio.h"
#include "klt.h"

// e-glue includes
#include "egluesh.h"
#include "egdebug.h"

// sift includes
#include "sift.h"
#include "imgfeatures.h"
#include "kdtree.h"
#include "utils.h"
#include "xform.h"

void PrintMat(CvMat *A);
eg_bool_t (*egFindTransform)(CvMat **H, IplImage *img1, IplImage *img2);
void egSetIdentity(CvMat *M);

extern long int timetable[MAX_TIME_TABLE_INDEX];

int main(int argc, char **argv)
{
    IplImage *ipl_img1, *ipl_img2;
    IplImage *newimg = NULL;
    IplImage *img_to_glue = NULL;
    char fnameout[100];
	CvMat *H, *H_to_glue, *H_disp;
	int usesift = 1; /* default */
	int useklt = 0;
	int opt;
	int n = 0;
	int total;
	CvSize xformed_size;
	int dispx, dispy;
	IplImage *xformed1;

    EG_INI();

    initTimeTable();

    while ((opt = getopt(argc, argv, "m:")) != -1)
    {
       switch (opt)
       {
       case 'm':
           if (!strncasecmp(optarg,"sift",4))
           {
              usesift = 1;
              useklt = 0;
           }
           if (!strncasecmp(optarg,"klt",3))
           {
           	usesift = 0;
           	useklt = 1;
           }
           break;
       default: /* ’?’ */
           fprintf(stderr, "Usage: %s [-m {sift|klt} filename*\n",
                   argv[0]);
           exit(EXIT_FAILURE);
       }
	}

	total = argc - optind;
    if (total == 0)
    {
        fprintf(stderr,"Usage: %s [-m {sift|klt} filename*\n", argv[0]);
        exit(EXIT_FAILURE);
    }
    printf("Processing %d files using ", total);
    if (useklt)
    {
    	printf("klt.\n");
    	egFindTransform = egFindTransformKLT;
    }
    if (usesift)
    {
    	printf("sift.\n");
    	egFindTransform = egFindTransformSIFT;
    }

    ipl_img1 = cvLoadImage( argv[optind], 1 );
    if ( ! ipl_img1 )
    {
        EG_ERR("Can't load img %s", argv[optind]);
        exit(EXIT_FAILURE);
    }
	n++;

    ipl_img2 = cvLoadImage( argv[optind+n], 1 );
    if ( ! ipl_img2 )
    {
        EG_ERR("Can't load img %s", argv[optind+n]);
        exit(EXIT_FAILURE);
    }
    n++;

    img_to_glue = ipl_img1;
    H_to_glue = cvCreateMat(3, 3, CV_64FC1);
    H_disp = cvCreateMat(3, 3, CV_64FC1);
    cvSetIdentity(H_to_glue, cvRealScalar(1));
    cvSetIdentity(H_disp, cvRealScalar(1));
    while ( ipl_img2 )
    {
        egGetSizeDisp(cvGetSize( ipl_img1 ), cvGetSize( ipl_img1 ), H_disp,
                  &xformed_size.width, &xformed_size.height, &dispx, &dispy);
        xformed1 = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
        cvWarpPerspective( ipl_img1, xformed1, H_disp,
                       CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS,
                       cvScalarAll( 0 ) );
	    if (egFindTransform(&H, xformed1, ipl_img2) == EG_TRUE)
		{
		    if (egGlue(&newimg, H_disp, img_to_glue, ipl_img2, H) == EG_TRUE)
		    {
				EG_INF("H_to_glue:\n");
				PrintMat(H_to_glue);
		    	printf("Image %d processed\n", n);
		    }
		    else
		    {
		        EG_INF("Cant glue images.");
		    }
		}
		else
		{
			EG_INF("Can't find the affine matrix (H).");
		}
		if (xformed1)
		{
			cvReleaseImage(&xformed1);
		}
	    if (ipl_img1 != img_to_glue)
	    {
	    	cvReleaseImage(&img_to_glue);
	    }
		cvReleaseImage(&ipl_img1);
		ipl_img1 = ipl_img2;
		ipl_img2 = NULL;
		if (n < total)
		{
		    ipl_img2 = cvLoadImage( argv[optind+n], 1 );
		    if ( ! ipl_img2 )
		    {
		    	if (newimg)
		    	{
			        sprintf(fnameout, "newimg.png");
			        EG_INF("Newimg created: %s\n", fnameout);
			        cvSaveImage(fnameout, newimg); /* Debugging. */
			        cvReleaseImage(&newimg);
		    	}
		        EG_ERR("Can't load img %s", argv[optind+n]);
		        exit(EXIT_FAILURE);
		    }
		    img_to_glue = newimg;
		    n++;
		}
		else
		{
			break;
		}
    }
    if (newimg)
    {
        sprintf(fnameout, "newimg.png");
        EG_INF("Newimg created: %s\n", fnameout);
        cvSaveImage(fnameout, newimg); /* Debugging. */
        cvReleaseImage(&newimg);
    }
//    cvReleaseMat(H_to_glue);
    showTimeTable();
    return 0;
}

/**
 * @returns the matches found.
 *
 */
int egFillPts(CvPoint2D64f *pts1, CvPoint2D64f *pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl)
{
    int i;
    int k = 0;
    for (i = 0 ; i < fl->nFeatures ; i++)
    {
        if (fl->feature[i]->val == 0)
        {
            printf("Feature #%d:  (%f,%f) with value of %d\n",
                   i, fl->feature[i]->x, fl->feature[i]->y, fl->feature[i]->val);
            pts1[k].x = ft->feature[i][frame]->x;
            pts1[k].y = ft->feature[i][frame]->y;
            pts2[k].x = fl->feature[i]->x;
            pts2[k].y = fl->feature[i]->y;
            k++;
        }
    }
    return k;
}

void PrintMat(CvMat *A)
{
    int i, j, tmp_i;
    char tmp[256], tmp2[256];
    EG_DEB( "Entering %s", __FUNCTION__);
    for (i = 0; i < A->rows; i++)
    {
        switch (CV_MAT_DEPTH(A->type))
        {
        case CV_32F:
        case CV_64F:
            memset(tmp, 0, 256);
            memset(tmp2, 0, 256);
            for (j = 0; j < A->cols; j++)
            {
                snprintf(tmp2, 256, "%8.3f ", (float)cvGetReal2D(A, i, j));
                tmp_i = strlen(tmp);
                strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
            }
            EG_INF(tmp);
            break;
        case CV_8U:
        case CV_16U:
            memset(tmp, 0, 256);
            memset(tmp2, 0, 256);
            for(j = 0; j < A->cols; j++)
            {
                snprintf(tmp2, 256, "%6d", (int)cvGetReal2D(A, i, j));
                tmp_i = strlen(tmp);
                strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
            }
            EG_INF(tmp);
            break;
        default:
            break;
        }
    }
    EG_DEB("Exiting %s", __FUNCTION__);
}

void egGetData(IplImage *ipl_img1, unsigned char **data, int *ncols, int *nrows)
{
    unsigned char *ipl_data = NULL;
    IplImage *tmp1 = NULL;
    CvSize size1;
    int x, y, step, i;
    tmp1 = cvCreateImage( cvGetSize(ipl_img1), IPL_DEPTH_8U, 3 );
    if ( ! tmp1 )
    {
        EG_ERR("Can't create tmp1 image");
        exit(1);
    }
    cvCvtColor( ipl_img1, tmp1, CV_RGB2YCrCb);
    cvGetRawData(ipl_img1, &ipl_data, &step, &size1);
    if ( ! ipl_data)
    {
        EG_ERR("Can't get ipl_data");
        exit(1);
    }
    EG_DEB("ipl_data[0]=%d size1.width=%d size1.height=%d step=%d\n",
           ipl_data[0], size1.width, size1.height, step);
    *data = malloc(size1.width*size1.height);
    if ( ! *data )
    {
        EG_ERR("Can't allocate data");
        exit(1);
    }
    i = 0;
    for( y = 0; y < size1.height; y++, ipl_data += step )
        for( x = 0; x < size1.width; x++ )
            (*data)[i++] = ipl_data[x*3];
    cvReleaseImage( &tmp1 );
    *ncols = size1.width;
    *nrows = size1.height;
}

eg_bool_t egGlue(IplImage **newimg, CvMat *H_disp, IplImage *img1, IplImage *img2, CvMat *H)
{
    IplImage* xformed2, *xformed1;
    int dispx, dispy; /* x and y displacement */
    CvMat *H_trans;
    IplImage* mask;
    struct timeval t1;
    static int toblend = 0;
    char toblend_name[256];

    CvSize xformed_size;
    if( H )
    {
    	tic(&t1);
        egGetSizeDisp(cvGetSize( img1 ), cvGetSize( img2 ), H,
                      &xformed_size.width, &xformed_size.height, &dispx, &dispy);
        /* resets the displacement the fit all the new image*/
    	H_trans = cvCreateMat(3, 3, H->type);
        egBuildH_disp(dispx, dispy, H_disp);
        egBuildH_trans(H, H_disp, H_trans);
        EG_DEB("\nH_trans:");
        PrintMat(H_trans);
        xformed2 = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
        xformed1 = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
        *newimg = cvCreateImage( xformed_size, IPL_DEPTH_8U, 3 );
        /* Ensure that the both images will be at the same coordinates system */
        cvWarpPerspective( img1, xformed1, H_trans,
                           CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS,
                           cvScalarAll( 0 ) );
        EG_DEB("After img1\n");
        cvWarpPerspective( img2, xformed2, H_disp,
                           CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS,
                           cvScalarAll( 0 ) );
        snprintf(toblend_name,256,"toblend_%03d.tif", toblend++);
        cvSaveImage(toblend_name, xformed2);
        EG_DEB("After img2\n");
        mask = egComputeMask(xformed1, xformed2);
        cvCopy( xformed1, *newimg, NULL );
        cvCopy( xformed2, *newimg, mask ); /* replaces only some pixels */
        cvReleaseImage( &xformed1 );
        cvReleaseImage( &xformed2 );
//        cvReleaseMat( &H_disp );
        cvReleaseMat( &H_trans );
        EG_DEB( "Exiting 2 %s", __FUNCTION__);
        timetable[GLUEIMAGE] += tac(t1);
        return EG_TRUE;
    }
    else
    {
        EG_DEB("Can't compute RANSAC.\n");
        EG_DEB( "Exiting 3 %s", __FUNCTION__);
        return EG_FALSE;
    }
}

IplImage* egComputeMask(IplImage *xformed1, IplImage *xformed2)
{
    IplImage *tmp1, *tmp2;
    EG_DEB( "Entering %s", __FUNCTION__);
    IplImage *mask = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 1);
    IplImage *tmp1arr = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 1);
    IplImage *tmp2arr = cvCreateImage( cvGetSize(xformed2), IPL_DEPTH_8U, 1);
    tmp1 = cvCreateImage( cvGetSize(xformed1), IPL_DEPTH_8U, 3 );
    tmp2 = cvCreateImage( cvGetSize(xformed2), IPL_DEPTH_8U, 3 );
    cvCvtColor( xformed1, tmp1, CV_RGB2YCrCb);
    cvCvtColor( xformed2, tmp2, CV_RGB2YCrCb);
    cvSplit(tmp1, tmp1arr, NULL, NULL, NULL);
    cvSplit(tmp2, tmp2arr, NULL, NULL, NULL);
    cvCmp( tmp1arr, tmp2arr, mask, CV_CMP_LT );
    cvReleaseImage( &tmp1 );
    cvReleaseImage( &tmp2 );
    cvReleaseImage( &tmp1arr );
    cvReleaseImage( &tmp2arr );
    EG_DEB( "Exiting %s", __FUNCTION__);
    return mask;
}

void egGetSizeDisp(CvSize size1, CvSize size2, CvMat* H_inv, int *width, int *height, int *dispx, int *dispy)
{
    int i;
    int minx, miny, maxx, maxy;
    EG_DEB( "Entering %s", __FUNCTION__);
    CvMat *new_dim = cvCreateMat(3, 4, H_inv->type);
    egGuessNewDim(size2, H_inv, new_dim);
    EG_DEB("\nnew_dim:");
    PrintMat(new_dim);
    minx = 0;
    miny = 0;
    maxx = size1.width;
    maxy = size1.height;
    for (i=1; i < new_dim->cols; i++)
    {
        if (minx > cvmGet(new_dim, 0, i))
        {
            minx = cvmGet(new_dim, 0, i);
        }
        if (maxx < cvmGet(new_dim, 0, i))
        {
            maxx = cvmGet(new_dim, 0, i);
        }
        if (miny > cvmGet(new_dim, 1, i))
        {
            miny = cvmGet(new_dim, 1, i);
        }
        if (maxy < cvmGet(new_dim, 1, i))
        {
            maxy = cvmGet(new_dim, 1, i);
        }
    }
    *height = maxy - miny;
    *width = maxx - minx;
    *dispx = minx;
    *dispy = miny;
    cvReleaseMat( &new_dim );
    EG_DEB("height = %d\n", *height);
    EG_DEB("width = %d\n", *width);
    EG_DEB("dispx = %d\n", *dispx);
    EG_DEB("dispy = %d\n", *dispy);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egNorm(CvMat* src, CvMat* dst)
{
    int i;
    EG_DEB( "Entering %s", __FUNCTION__);
    assert(src->rows == 3);
    for (i=0; i < src->cols; i++)
    {
        double factor = cvmGet(src, 2, i);
        cvmSet(dst, 0, i, cvmGet(src, 0, i) / factor);
        cvmSet(dst, 1, i, cvmGet(src, 1, i) / factor);
        cvmSet(dst, 2, i, 1.0);
    }
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egNorm3x3(CvMat* src, CvMat* dst)
{
    int i,j;
    double factor;
    EG_DEB( "Entering %s", __FUNCTION__);
    EG_DEB("rows = %d\n cols = %d", src->rows, src->cols);
    assert(src->rows == 3 && src->cols == 3);
    factor = cvmGet(src, 2, 2);
    /* TODO: look for a matrix to scalar multiplication. */
    for (i=0; i < src->cols; i++)
    {
        for (j=0; j < src->rows; j++)
        {
            cvmSet(dst, j, i, cvmGet(src, j, i) / factor);
        }
    }
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egGuessNewDim(CvSize size, CvMat* H_inv, CvMat* new_dim)
{
    EG_DEB( "Entering %s", __FUNCTION__);
    CvMat *dim = cvCreateMat(3, 4, H_inv->type);
    cvmSet(dim, 0, 0, 0);
    cvmSet(dim, 1, 0, 0);
    cvmSet(dim, 2, 0, 1);
    cvmSet(dim, 0, 1, size.width);
    cvmSet(dim, 1, 1, 0);
    cvmSet(dim, 2, 1, 1);
    cvmSet(dim, 0, 2, 0);
    cvmSet(dim, 1, 2, size.height);
    cvmSet(dim, 2, 2, 1);
    cvmSet(dim, 0, 3, size.width);
    cvmSet(dim, 1, 3, size.height);
    cvmSet(dim, 2, 3, 1);
    cvMatMul(H_inv, dim, new_dim);
    egNorm(new_dim, new_dim);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egBuildH_disp(int dispx, int dispy, CvMat* H_disp)
{
    EG_DEB( "Entering %s", __FUNCTION__);
    cvmSet(H_disp, 0, 0, 1.0);
    cvmSet(H_disp, 0, 1, 0);
    cvmSet(H_disp, 0, 2, -dispx);
    cvmSet(H_disp, 1, 0, 0);
    cvmSet(H_disp, 1, 1, 1.0);
    cvmSet(H_disp, 1, 2, -dispy);
    cvmSet(H_disp, 2, 0, 0);
    cvmSet(H_disp, 2, 1, 0);
    cvmSet(H_disp, 2, 2, 1.0);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egBuildH_trans(CvMat* H_inv, CvMat* H_disp, CvMat* H_trans)
{
    EG_DEB( "Entering %s", __FUNCTION__);
    cvMatMul(H_inv, H_disp, H_trans);
    egNorm3x3(H_trans, H_trans);
    EG_DEB( "Exiting %s", __FUNCTION__);
}

void egDrawLines(IplImage *stacked, CvPoint2D64f *pts1, CvPoint2D64f *pts2, int npts)
{
    CvPoint pt1, pt2;
    int i;
    int height = stacked->height / 2;
    for (i=0; i<npts; i++)
    {
        pt1 = cvPoint( cvRound( pts1[i].x ), cvRound( pts1[i].y ) );
        pt2 = cvPoint( cvRound( pts2[i].x ), cvRound( pts2[i].y + height) );
        cvLine( stacked, pt1, pt2, CV_RGB(255,255,0), 1, 8, 0 );
    }
}

void egShowTc( KLT_TrackingContext tc )
{
    printf("KLT_TrackingContext\n");  /* minimum distance between selected features */
    printf("mindist                =\t%d\n", tc->mindist);
    printf("window_width           =\t%d\n", tc->window_width);         /* dimensions of feature window */
    printf("window_height          =\t%d\n", tc->window_height);
    printf("sequentialMode         =\t%d\n", tc->sequentialMode);  /* whether to save most recent image */
    printf("smoothBeforeSelecting  =\t%d\n", tc->smoothBeforeSelecting); /* whether to smooth image before selecting features */
    printf("writeInternalImages    =\t%d\n", tc->writeInternalImages);   /* whether to write internal images for later viewing */
    printf("min_eigenvalue         =\t%d\n", tc->min_eigenvalue);       /* smallest eigenvalue allowed for selecting */
    printf("min_determinant        =\t%f\n", tc->min_determinant);    /* min determinant for declaring tracking failure */
    printf("min_displacement       =\t%f\n", tc->min_displacement);   /* amount of pixel shift for stopping tracking iterations */
    printf("max_iterations         =\t%d\n", tc->max_iterations);       /* max iterations before declaring tracking failure */
    printf("max_residue            =\t%f\n", tc->max_residue);        /* max residue before declaring tracking failure */
    printf("grad_sigma             =\t%f\n", tc->grad_sigma);         /* sigma of gaussian for computing gradient */
    printf("smooth_sigma_fact      =\t%f\n", tc->smooth_sigma_fact);  /* sigma factor of gaussian for smoothing image */
    printf("pyramid_sigma_fact     =\t%f\n", tc->pyramid_sigma_fact); /* sigma factor of gaussian for computing image pyramid */
    printf("nSkippedPixels         =\t%d\n", tc->nSkippedPixels);       /* used to speed up feature selection */
    printf("borderx                =\t%d\n", tc->borderx);              /* border in which features will not be selected, and */
    printf("bordery                =\t%d\n", tc->bordery);              /*    tracked features will be declared out-of-bounds */
    printf("nPyramidLevels         =\t%d\n", tc->nPyramidLevels);       /* number of pyramid levels */
    printf("subsampling            =\t%d\n", tc->subsampling);          /* amount of subsampling between pyramid levels */
}

int egFindMatches(CvPoint2D64f *matched1, CvPoint2D64f *matched2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl)
{
    int num;
    CvMat *status;
    CvMat *points1, *points2;
    int i,k=0;
    CvMat *fundamental_matrix;

    k = egFillPtsMat(&points1, &points2, ft, frame, fl);
    if (k <= 4)
    {
        EG_INF("Can't compute enough matches.");
        return k;
    }
    EG_DEB("Total KLT matches = %d", k);
    /* Fill the points here ... */
    status   = cvCreateMat(1,points1->cols,CV_32F);
    fundamental_matrix = cvCreateMat(3,3,CV_32F);
    //	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
    num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.5,0.99,status);
    //	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.2,0.99,status);
    //	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.5,0.50,status);
    if( num == 1 )
    {
        k = 0;
        for (i=0; i<status->cols; i++)
        {
            if ( cvmGet(status,0,i) == 1.0 )
            {
                k++;
            }
        }
        EG_DEB("Total cvFindFundamentalMat matches = %d", k);
        EG_DEB("status:");
        PrintMat(status);
        k = 0;
        for (i=0; i<status->cols; i++)
        {
            if ( cvmGet(status,0,i) == 1.0 )
            {
                matched1[k].x = cvmGet(points1, 0, i);
                matched1[k].y = cvmGet(points1, 1, i);
                matched2[k].x = cvmGet(points2, 0, i);
                matched2[k].y = cvmGet(points2, 1, i);
                EG_DEB("matched1[%d]=%f,%f", k, matched1[k].x, matched1[k].y);
                EG_DEB("matched2[%d]=%f,%f", k, matched2[k].x, matched2[k].y);
                k++;
            }
        }
    }
    else
    {
        k = 0;
        EG_INF("Fundamental matrix was not found\n");
    }
    return k;
}

/**
 * @returns the matches found.
 *
 */
int egFillPtsMat(CvMat **pts1, CvMat **pts2, KLT_FeatureTable ft, int frame, KLT_FeatureList fl)
{
    int i;
    int k = 0;
    for (i = 0 ; i < fl->nFeatures ; i++)
    {
        if (fl->feature[i]->val == 0)
        {
            k++;
        }
    }
    *pts1 = cvCreateMat(3,k,CV_32FC1);
    *pts2 = cvCreateMat(3,k,CV_32FC1);
    k = 0;
    for (i = 0 ; i < fl->nFeatures ; i++)
    {
        if (fl->feature[i]->val == 0)
        {
            cvmSet(*pts1,0,k,ft->feature[i][frame]->x);
            cvmSet(*pts1,1,k,ft->feature[i][frame]->y);
            cvmSet(*pts1,2,k,1);
            cvmSet(*pts2,0,k,fl->feature[i]->x);
            cvmSet(*pts2,1,k,fl->feature[i]->y);
            cvmSet(*pts2,2,k,1);
            k++;
        }
    }
    return k;
}

/**
 * Returns in H a transformation matrix (4x4) representing the transform that takes
 * the img1 to same position as img2. The similarity between the images are get using
 * Kanade-Lucas-Tomasi method. This method is effective only when images are close
 * each other and with some "corners" and others precise features to be tracked. This
 * algorithm predicts the use a image sequence and, then, new features should be
 * inserted as new images are processed. This function doesn't implement this feature.
 * TODO: investigate how to implement the "replace" tracking points.
 * @param H matrix to be filled with the homogeneous transform.
 * @param img1 The first image to be compared (source)
 * @param img2 The second image to be compared (target)
 */
eg_bool_t egFindTransformKLT(CvMat **H, IplImage *img1, IplImage *img2)
{
    KLT_TrackingContext tc;
    KLT_FeatureList fl;
    KLT_FeatureTable ft;
#ifdef DEBUG
    IplImage *stacked;
#endif
    unsigned char *ipl_img1_data;
    unsigned char *ipl_img2_data;
    int nFeatures = NFEATURES, nFrames = 2;
    CvPoint2D64f pts1[NFEATURES], pts2[NFEATURES];
    int ncols, nrows;
    int npts = 0;

    tc = KLTCreateTrackingContext();
    tc->mindist = 10; /* default 10 */
    //  egShowTc(tc);
    fl = KLTCreateFeatureList(nFeatures);
    ft = KLTCreateFeatureTable(nFrames, nFeatures);
    tc->sequentialMode = TRUE;
    tc->writeInternalImages = FALSE;
//    tc->affineConsistencyCheck = -1;  /* set this to 2 to turn on affine consistency check */
    tc->affineConsistencyCheck = 2;  /* set this to 2 to turn on affine consistency check */

    egGetData(img1, &ipl_img1_data, &ncols, &nrows);
    if( ! ipl_img1_data )
    {
        EG_ERR( "unable to get data from img1");
        exit(1);
    }

    KLTSelectGoodFeatures(tc, ipl_img1_data, ncols, nrows, fl);
    KLTStoreFeatureList(fl, ft, 0);
    KLTWriteFeatureListToPPM(fl, ipl_img1_data, ncols, nrows, "feat001.ppm");

    egGetData(img2, &ipl_img2_data, &ncols, &nrows);
    if( ! ipl_img2_data )
    {
        EG_ERR( "unable to get data from img2");
        exit(1);
    }

    KLTTrackFeatures(tc, ipl_img1_data, ipl_img2_data, ncols, nrows, fl);

    EG_DEB("nFeatures=%d", fl->nFeatures);
    npts = egFindMatches(pts1, pts2, ft, 0, fl);
    EG_DEB("npts=%d",npts);
    if ( npts <= 4 )
    {
        free(ipl_img1_data);
        free(ipl_img2_data);
        EG_INF("Can't compute fundamental matches.");
        return EG_FALSE;
    }

    *H = lsq_homog( pts1, pts2, npts );
    if (NULL != H)
    {
        EG_INF("Affine:");
        PrintMat(*H);
    }
    else
    {
        fprintf(stderr,"Can't find lsq_homog in %d pts.\n", npts);
    }

#ifdef DEBUG
    stacked = stack_imgs( img1, img2 );
    /* Debugging. */
    egDrawLines(stacked, pts1, pts2, npts);
    cvNamedWindow( "Xformed", 1 );
    cvShowImage( "Xformed", stacked );
    cvSaveImage("stacked.png", stacked);
    cvWaitKey( 0 );
#endif

//#ifdef REPLACE
//    KLTReplaceLostFeatures(tc, ipl_img2_data, ncols, nrows, fl);
//#endif
//
//    KLTStoreFeatureList(fl, ft, i-1);
//    sprintf(fnameout, "feat%03d.ppm", i);
//    KLTWriteFeatureListToPPM(fl, ipl_img2_data, ncols, nrows, fnameout);
//    cvReleaseImage(&ipl_img2);
//    ipl_img2_data = NULL;
//
    KLTFreeFeatureTable(ft);
    KLTFreeFeatureList(fl);
    KLTFreeTrackingContext(tc);
    free(ipl_img1_data);
    free(ipl_img2_data);
    return EG_TRUE;
}

/**
 * Find features based on two input images.
 *
 * Note that this line:
 *
 * feat1[i].fwd_match = nbrs[0];
 *
 * is important for the RANSAC function to work.
 *
 */
int egFindFeatures(IplImage *img1, feature_t** feat1, int *n1, IplImage *img2, feature_t **feat2)
{
#ifdef DEBUG
	IplImage *stacked;
#endif
    feature_t *feat;
	struct kd_node* kd_root;
    double d0, d1;
	feature_t **nbrs;
    CvPoint pt1, pt2;
	int i, k, m = 0;
	int n2;
	struct timeval t1,t2;
	tic(&t2);
    EG_DEB( "Entering %s", __FUNCTION__);
#ifdef DEBUG
	stacked = stack_imgs( img1, img2 );
#endif
	tic(&t1);
	*n1 = _sift_features( img1, feat1, SIFT_INTVLS, SIFT_SIGMA, EG_SIFT_CONTR_THR,
			 EG_SIFT_CURV_THR, SIFT_IMG_DBL, SIFT_DESCR_WIDTH,
			 SIFT_DESCR_HIST_BINS );
	timetable[SIFT1] += tac(t1);
	tic(&t1);
	n2 = _sift_features( img2, feat2, SIFT_INTVLS, SIFT_SIGMA, EG_SIFT_CONTR_THR,
			 EG_SIFT_CURV_THR, SIFT_IMG_DBL, SIFT_DESCR_WIDTH,
			 SIFT_DESCR_HIST_BINS );
 	timetable[SIFT2] += tac(t1);
	tic(&t1);
    kd_root = kdtree_build( *feat2, n2 );
    for( i = 0; i < *n1; i++ )
    {
        feat = *feat1 + i;
        k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
        if( k == 2 )
        {
            d0 = descr_dist_sq( feat, nbrs[0] );
            d1 = descr_dist_sq( feat, nbrs[1] );
            if( d0 < d1 * NN_SQ_DIST_RATIO_THR )
            {
                pt1 = cvPoint( cvRound( feat->x ), cvRound( feat->y ) );
                pt2 = cvPoint( cvRound( nbrs[0]->x ), cvRound( nbrs[0]->y ) );
                pt2.y += img1->height;
#ifdef DEBUG
                cvLine( stacked, pt1, pt2, CV_RGB(255,0,255), 1, 8, 0 );
#endif
                m++;
                (*feat1)[i].fwd_match = nbrs[0];
            }
        }
        free( nbrs );
    }
	timetable[KDTREE] += tac(t1);
    EG_DEB("Found %d total matches\n", m );
#ifdef DEBUG
//    display_big_img( stacked, "Matches" );
    cvSaveImage("stacked.png", stacked);
//    cvWaitKey( 0 );
    cvReleaseImage( &stacked );
#endif
    EG_DEB("After release stacked\n");
    kdtree_release( kd_root );
    EG_DEB( "Exiting %s", __FUNCTION__);
    timetable[FINDFEATURES] += tac(t2);
    return m;
}

/**
 * Returns in H a transformation matrix (4x4) representing the transform that takes
 * the img1 to same position as img2. The similarity between the images are get using
 * Scale Invariant Features Tracking (SIF).
 * @param H matrix to be filled with the homogeneous transform.
 * @param img1 The first image to be compared (source)
 * @param img2 The second image to be compared (target)
 */
eg_bool_t egFindTransformSIFT(CvMat **H, IplImage *img1, IplImage *img2)
{
    feature_t *feat1, *feat2;
    int m, n1;
    struct timeval t1;
	m = egFindFeatures(img1, &feat1, &n1, img2, &feat2);
	tic(&t1);
	*H = ransac_xform( feat1, n1, FEATURE_FWD_MATCH, lsq_homog, EG_MIN_RANSAC_MATCHES, 0.01,
                      homog_xfer_err, 3.0, NULL, NULL );
	timetable[RANSAC] += tac(t1);
    free( feat1 );
    free( feat2 );
    if ( *H )
    {
    	return EG_TRUE;
    }
    else
    {
    	return EG_FALSE;
    }
}

/**
 * Set an identity matrix using a loop. TODO: look for a faster way to create a identity matrix.
 * @param M The matrix to be modified.
 */
void egSetIdentity(CvMat *M)
{
	int i;
	int j;
	for (i=0; i<M->cols; i++)
	{
		for (j=0; j<M->rows; j++)
		{
			if (i != j)
			{
				cvmSet(M, j, i, 0);
			}
			else
			{
				cvmSet(M, j, i, 1);
			}
		}
	}
}
